package edu.krazy.engine;

/**
 * @(#)Utils.java
 *
 *
 * @author: Sr Pr0n 
 * @version 1.00 2008/10/22
 */


public class Utils {

    public static final int NORTH = 0;
    public static final int SOUTH = 1;
    
    public static float[]  calculateNormal3f(Face f){
    	//System.out.println("calculating normal");
    	// (v1-v2)x(v2-v3)
    	float[] temp1 = new float[3];
    	float[] temp2 = new float[3];
    	//calculate temp1
    	temp1[0]= f.getV1().getX()-f.getV2().getX();
    	temp1[1]= f.getV1().getY()-f.getV2().getY();
    	temp1[2]= f.getV1().getZ()-f.getV2().getZ();
    	
    	//calculate temp2
    	temp2[0]= f.getV2().getX()-f.getV3().getX();
    	temp2[1]= f.getV2().getY()-f.getV3().getY();
    	temp2[2]= f.getV2().getZ()-f.getV3().getZ();
    	
    	
    	return normalize(crossProduct(temp1,temp2));
    	
    }
    
    
    public static float dotProduct(Vertex3f v1, Vertex3f v2)
    {
    	return v1.getX()*v2.getX()+v1.getY()*v2.getY()+v1.getZ()*v2.getZ();
    	
    }
    
    public static float dotProduct(Vertex2f v1, Vertex2f v2)
    {
    	return v1.getX()*v2.getX()+v1.getY()*v2.getY();
    	
    }
    public static float[] crossProduct(float[] v1, float[] v2){
    	//System.out.println("calculating cross product");
    /*
     * x1	  x2	(y1*z2)-(z1*y2)
     * y1  x  y2 =  (z1*x2)-(x1*z2)
     * z1	  z2    (x1*y2)-(y1*x2)
     */
     float[] result = new float[3];
     
     result[0] = v1[1]*v2[2]- v1[2]*v2[1];
     result[1] = v1[2]*v2[0]- v1[0]*v2[2];
     result[2] = v1[0]*v2[1]- v1[1]*v2[0];
     
     return result;	
    }
    
       public static Vertex3f crossProduct(Vertex3f v1, Vertex3f v2){
    	//System.out.println("calculating cross product");
    /*
     * x1	  x2	(y1*z2)-(z1*y2)
     * y1  x  y2 =  (z1*x2)-(x1*z2)
     * z1	  z2    (x1*y2)-(y1*x2)
     */
     float[] result = new float[3];
     
       result[0] = v1.getY()*v2.getZ()-v1.getZ()*v2.getY();
       result[1] = v1.getZ()*v2.getX()-v1.getX()*v2.getZ();
       result[2] = v1.getX()*v2.getY()-v1.getY()*v2.getX();
     
     
     return new Vertex3f(result[0],result[1],result[2]);	
    }
    
    public static Vertex3f normalize(Vertex3f v)
    {
    	float mag = (float)Math.sqrt( v.getX()*v.getX()+v.getY()*v.getY()+v.getZ()*v.getZ());
        
        return new Vertex3f( v.getX()/mag, v.getY()/mag, v.getZ()/mag );
    	
    }
    public static float[] normalize(float[] v){
    	
    float mag =(float)Math.sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
    
    float result[] = new float[3];
    
    result[0]=v[0]/mag;
    result[1]=v[1]/mag;
    result[2]=v[2]/mag;
    return result;	
    }
    //rotate2fVector(float v[], float angle);
    
     public static float[] normalize2f(float[] v){
    	
    float mag =(float)Math.sqrt(v[0]*v[0]+v[1]*v[1]);
    
    float result[] = new float[2];
    
    result[0]=v[0]/mag;
    result[1]=v[1]/mag;
    
    return result;	
    }
    
    
    public static Vertex3f getDirectionVector(Vertex3f v1, Vertex3f v2)
    {
    	
    	return new Vertex3f(v2.getX()-v1.getX(),v2.getY()-v1.getY(),v2.getZ()-v1.getZ());
    }
    
       
    public static Vertex2f getDirectionVector(Vertex2f v1, Vertex2f v2)
    {
    	
    	return new Vertex2f(v2.getX()-v1.getX(),v2.getY()-v1.getY());
    }
    
    public static float getDistance(Vertex3f v0, Vertex3f v1)
    {
    	
    	return (float) Math.sqrt((v1.getX()-v0.getX())*(v1.getX()-v0.getX())+(v1.getZ()-v0.getZ())*(v1.getZ()-v0.getZ()));
    }
    
    public static float[] rotateVector2f(float v[],float angle)
    {
    	
    		/*
    	 *  | x | * | cos@  sin@ | =  (x*cos@)+(z*-sin@)
    	 *  | z |   |-sin@  cos@ | =  (x*sin@)+(z*cos@)
    	 *
    	 */
    	 
    	//asumiendo que angle son radianes
    	//get distance Vector
       float tmp[] = new float[2];
       float distanceVector[] = new float[2];
       float resultVector[] = new float[2];
       
      
       distanceVector = v;
       //System.out.println("distance vector is: "+ distanceVector[0]+" "+ distanceVector[1]);
       //rotate distance vector the given angle
       resultVector[0] = distanceVector[0]*(float)Math.cos(angle)+distanceVector[1]*-(float)Math.sin(angle);
       resultVector[1] = distanceVector[0]*(float)Math.sin(angle)+distanceVector[1]*(float)Math.cos(angle);
       return resultVector;
    }
    
    public static boolean isCollidingWithKart(Kart3D kart1, Kart3D kart2, float sphere_radius)
    {
    	//usar bounding metodo sphere
    	float distance = (float)Math.sqrt ( (kart1.getPosition().getX()-kart2.getPosition().getX())*(kart1.getPosition().getX()-kart2.getPosition().getX())+
    									   (kart1.getPosition().getY()-kart2.getPosition().getY())*(kart1.getPosition().getY()-kart2.getPosition().getY())+
    		    							(kart1.getPosition().getZ()-kart2.getPosition().getZ())*(kart1.getPosition().getZ()-kart2.getPosition().getZ()) 
    		    							);
    	if(distance<2*sphere_radius)	    	
    		    	return true;	
    	else
    		return false;	    		
    }
    
    public static Vertex3f subVector3f(Vertex3f v1, Vertex3f v2)
    {
    	return new Vertex3f(v1.getX()-v2.getX(),v1.getY()-v2.getY(),v1.getZ()-v2.getZ());
    	
    }
    
     public static Vertex3f addVector3f(Vertex3f v1, Vertex3f v2)
    {
    	return new Vertex3f(v1.getX()+v2.getX(),v1.getY()+v2.getY(),v1.getZ()+v2.getZ());
    	
    }
    public static Vertex3f multVector3fByScalar(Vertex3f v, float s)
    {
    	
    	return new Vertex3f(v.getX()*s,v.getY()*s,v.getZ()*s);
    	
    }
    public static void setKartBounce(Kart3D kart1, Kart3D kart2)
    	
    {
    	
     //First, find the normalized vector n from the center of 
    // circle1 to the center of circle2
    
    Vertex3f n = new Vertex3f( kart1.getPosition().getX()-kart2.getPosition().getX(),kart1.getPosition().getY()-kart2.getPosition().getY(),kart1.getPosition().getZ()-kart2.getPosition().getZ());
	n=new Vertex3f(normalize(n));
    // Find the length of the component of each of the movement vectors along n. 
   // Vertex3f directionVector1 = new Vertex3f(kart1.target.getX()-kart1.getPosition().getX(),0f,kart1.target.getZ()-kart1.getPosition().getZ());
    //Vertex3f directionVector2 = new Vertex3f(kart2.target.getX()-kart2.getPosition().getX(),0f,kart2.target.getZ()-kart2.getPosition().getZ());
    
    Vertex3f directionVector1 = kart1.accelVector;
    Vertex3f directionVector2= new Vertex3f(kart2.accelVector.getX()*.1f,0f,kart2.accelVector.getZ()*.1f); 
    float a1 = dotProduct(directionVector1,n);
    float a2 = dotProduct(directionVector2,n);
    
    float optimizedP = (a1-a2);
	// Calculate v1', the new movement vector of circle1
   // v1' = v1 - optimizedP * m2 * n  //mass = 1
   
    
    Vertex3f tmp1 = multVector3fByScalar(n,optimizedP);
    Vertex3f v1 = subVector3f(directionVector1,tmp1);
    
	//SET NEW DIRECTION VECTOR
	kart1.accelVector = v1;
	//kart1.target=new Vertex3f(kart1.getPosition().getX()+v1.getX(),0f,kart1.getPosition().getZ()+v1.getZ());
    }
    public static boolean sameSide(Vertex3f p1, Vertex3f p2, Vertex3f a, Vertex3f b)
    	
    {
    	
    	Vertex3f cp1 = crossProduct(subVector3f(b,a),subVector3f(p1,a));
    	Vertex3f cp2 = crossProduct(subVector3f(b,a),subVector3f(p2,a));
    	
    	if (dotProduct(cp1,cp2)>=0)
    		return true;
    	else
    		return false;	
    }
    
    public static boolean pointInTriangle(Vertex3f p, Face f)
    {
    	
    	Vertex3f a = f.getV1();
    	Vertex3f b = f.getV2();
    	Vertex3f c = f.getV3();
    
    	
    	if( sameSide(p,a,b,c)&&sameSide(p,b,a,c)&&sameSide(p,c,a,b))
    		return true;
    	else
    		return false;	
    	
    	
    }
    
    public static float getAngleBetweenVectors(Vertex3f v1, Vertex3f v2)
    {
    	v1 = normalize(v1);
    	v2 = normalize(v2);
    	
    	
    	return (float)Math.toDegrees(Math.acos(dotProduct(v1,v2)));
    }
    public static boolean pointInTriangleTest(Vertex3f p, Face f)
    {
    	//get 3 vectors from point to triangle vertices
    	Vertex3f v1 = f.getV1();
    	v1.setY(0f);
    	Vertex3f v2 = f.getV2();
    	v2.setY(0f);
    	Vertex3f v3 = f.getV3();
    	v3.setY(0f);
    	p.setY(0f);
    	
    	//System.out.println("test p("+p.getX()+" "+p.getY()+" "+p.getZ()+") f(V1("+v1.getX()+" "+v1.getY()+v1.getZ()+
    	//	" V2("+v2.getX()+" "+v2.getY()+" "+v2.getZ()+"), V3("+v3.getX()+" "+v3.getY()+" "+v3.getZ());
    	                          
    	Vertex3f A = normalize(getDirectionVector(p,v1));
    	Vertex3f B = normalize(getDirectionVector(p,v2));
    	Vertex3f C = normalize(getDirectionVector(p,v3));
    	
    	//get angle between these vectors
    	
    	float a1 = getAngleBetweenVectors(A,B);
    	float a2 = getAngleBetweenVectors(B,C);
    	float a3 = getAngleBetweenVectors(C,A);
    	float result = a1+a2+a3;
    	//System.out.println("angles are " +a1+" "+a2+" "+a3 +" total "+result);
    	//System.out.println("total angle is: "+result);
    	//if the total is more than 360 then its outside
    	float offset = 0.1f;
    	if((360-offset)<=result)
    	{
    		
    		return true;
    	}
    		
    	else{
    		
    		return false;
    		
    	}
    				
    }
    
    public static boolean pointIn2DTrianleTest(Vertex3f p,Face f )
    {
    	Vertex2f Tpt = new Vertex2f(p.getX(),p.getZ());
    	Vertex2f[] trianglePoints = new Vertex2f[3];
    	trianglePoints[0] = new Vertex2f(f.getV1().getX(),f.getV1().getZ());
    	trianglePoints[1] = new Vertex2f(f.getV2().getX(),f.getV2().getZ());
    	trianglePoints[2] = new Vertex2f(f.getV3().getX(),f.getV3().getZ());
    	
    	Vertex2f[] edges = new Vertex2f[3];
    	Vertex2f point2D = new Vertex2f(p.getX(),p.getZ());
    	edges[0]= getDirectionVector(trianglePoints[0],trianglePoints[1]);
    	edges[1]= getDirectionVector(trianglePoints[1],trianglePoints[2]);
    	edges[2]= getDirectionVector(trianglePoints[2],trianglePoints[0]);
    	
    	for(int i =0; i<3; i++)
    	{
    		//make edge normal NEi from Edge Ei
    		Vertex2f NEi = new Vertex2f(edges[i].getY(),-edges[i].getX());
    		//make Vi
    		Vertex2f Vi = getDirectionVector(Tpt,trianglePoints[i]);
    		
    		if(dotProduct(NEi,Vi)>0)
    			return false;
    		
    		
    	}
    
    	return true;
    }
    
    public static boolean pointInMesh(Vertex3f p, Model3D mesh)
    {
    	
    	p.setY(0f);
    	for(int i = 0; i < mesh.faceList.length; i++)
    	{
    		
    		Face f = mesh.faceList[i];
    		f.getV1().setY(0f);
    		f.getV2().setY(0f);
    		f.getV3().setY(0f);
    		
    	
    		//System.out.print("face "+(i+1)+" ");
    		if(pointIn2DTrianleTest(p,f))
    		{
    			//
    			//System.out.println("point "+p.getX()+" "+p.getY()+" "+p.getZ()+" in face "+(i+1));
    			return true;
    			
    		}
    			
    	}
    	
    	//System.out.println("point "+p.getX()+" "+p.getY()+" "+p.getZ()+" not in mesh");
    	return false;
    }
    
}
